#include "codec.h"
#include "utils/byte_queue.h"
#include "utils/crc16.h"
#include "utils/utils.h"

static void encoder_tx_start(encoder_t *enc);
static void encoder_tx_continue(encoder_t *enc, const void *buff, u16 length);
static void encoder_tx_end(encoder_t *enc);

static bool _on_rx_frame(decoder_t *dec)
{
	u16 crc0 = decode_u16(dec->rx_frame + dec->rx_length);
	u16 crc1 = crc16_get(dec->rx_frame, dec->rx_length);

	if (crc0 != crc1 || dec->rx_length < 1) {
		return false;
	}
	dec->call_back(dec->rx_frame, dec->rx_length);
	return true;
}
void codec_decoder_callback(void *param, u8 *buff, int size) {
    decoder_t *dec = (decoder_t *)param;
	for (u8 *buff_end = buff + size; buff < buff_end; buff++) {
		u8 data = *buff;
		switch(data){
			case CH_START:
				dec->rx_length = 0;
				dec->escape = false;
				dec->start = true;
				break;
			case CH_END:
				if (dec->rx_length > 2 && dec->rx_length != 0xFFFF){
					dec->rx_length -= 2; //skip crc
					_on_rx_frame(dec);
				}
				dec->rx_length = 0xFFFF;
				dec->start = false;
				break;
			case CH_ESC:
				dec->escape = true;
				break;
			default:
				if (dec->escape) {
					dec->escape = false;
					switch (data) {
						case CH_ESC_START:
							data = CH_START;
							break;

						case CH_ESC_END:
							data = CH_END;
							break;

						case CH_ESC_ESC:
							data = CH_ESC;
							break;

						default:
							data = 0xFF;
					}
				}

				if (dec->rx_length < sizeof(dec->rx_frame)) {
					dec->rx_frame[dec->rx_length] = data;
					dec->rx_length++;
				} else {
					dec->rx_length = 0xFFFF;
				}		
		}
	}
}

void codec_init_decoder(decoder_t *decoder) {
    decoder->escape = false;
    decoder->rx_index = decoder->rx_index = 0;
}

void codec_init_encoder(encoder_t *encoder) {
    encoder->crc = 0;
    byte_queue_init(&encoder->tx_queue, encoder->tx_cache, sizeof(encoder->tx_cache));
}

void codec_encode_message(encoder_t *encoder, u8 *head, int head_size, u8 *data, int len) {
    encoder_tx_start(encoder);
	encoder_tx_continue(encoder, head, head_size);
    encoder_tx_continue(encoder, data, len);
    encoder_tx_end(encoder);
}

static u16 encoder_write_byte(encoder_t *enc, u8 value)
{
	return byte_queue_write(&enc->tx_queue, &value, 1);
}

static void encoder_write_byte_esc(encoder_t *enc, u8 value)
{
	switch (value) {
	case CH_START:
		encoder_write_byte(enc, CH_ESC);
		value = CH_ESC_START;
		break;

	case CH_END:
		encoder_write_byte(enc, CH_ESC);
		value = CH_ESC_END;
		break;

	case CH_ESC:
		encoder_write_byte(enc, CH_ESC);
		value = CH_ESC_ESC;
		break;
	}

	encoder_write_byte(enc, value);
}

static void encoder_write_esc(encoder_t *enc, const u8 *buff, u16 length)
{
	const u8 *buff_end;

	for (buff_end = buff + length; buff < buff_end; buff++) {
		encoder_write_byte_esc(enc, *buff);
	}
}

static void encoder_tx_start(encoder_t *enc)
{
	enc->crc = 0;
	encoder_write_byte(enc, CH_START);
}

static void encoder_tx_continue(encoder_t *enc, const void *buff, u16 length)
{
	encoder_write_esc(enc, (const u8 *) buff, length);
	enc->crc = crc16_update(enc->crc, (const u8 *) buff, length);
}

static void encoder_tx_end(encoder_t *enc)
{
	encoder_write_esc(enc, (u8 *)&enc->crc, sizeof(enc->crc));
	encoder_write_byte(enc, CH_END);
}